#include "hnurm_dart/detect_node.hpp"


#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.hpp>
namespace hnurm
{
DetectNode::DetectNode(const rclcpp::NodeOptions &options) : rclcpp::Node("detect_node", options)
{
    init_params();
    // subs
    sub_img_ = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::Image>>(
        this, image_topic_, rmw_qos_profile_sensor_data
    );
    sub_uart_ = std::make_shared<message_filters::Subscriber<hnurm_interfaces::msg::VisionRecvData>>(
        this, recv_topic_, rmw_qos_profile_sensor_data
    );
    // sync_ = std::make_shared<message_filters::Synchronizer<ApproximateTimePolicy>>(
    //     ApproximateTimePolicy(4'096), *sub_img_, *sub_uart_
    // );
    // sync_->registerCallback(&DetectNode::detect_callback, this);
    // // pubs
    pub_res_img_ = image_transport::create_publisher(this, res_img_topic_, rmw_qos_profile_sensor_data);
    pub_bin_img_ = image_transport::create_publisher(this, bin_img_topic_, rmw_qos_profile_sensor_data);
    // // debug pubs
    std::string debug_topic = this->declare_parameter("debug_info_topic", "/debug/detect_time");
    pub_detect_time_        = this->create_publisher<std_msgs::msg::Float64>(debug_topic, rclcpp::SensorDataQoS());
    pub_send_data_        = this->create_publisher<hnurm_interfaces::msg::VisionSendData>(send_topic_, rclcpp::ServicesQoS());
    auto qos = rclcpp::QoS(rclcpp::KeepLast(10)).best_effort();
    image_sub_ = this->create_subscription<sensor_msgs::msg::Image>(
            "image",qos,std::bind(&DetectNode::image_callback, this, std::placeholders::_1));
    cv::namedWindow("res");

    cv::namedWindow("bin");
}

void DetectNode::init_params()
{
    bias = this->declare_parameter("bias", 0.0);
        // topics
    recv_topic_  = this->declare_parameter("vision_recv_topic", "vision_recv_data");
    send_topic_  = this->declare_parameter("vision_send_topic", "vision_send_data");
    image_topic_     = this->declare_parameter("img_topic", "image");
    res_img_topic_   = this->declare_parameter("res_img_topic", "res_img");
    bin_img_topic_   = this->declare_parameter("bin_img_topic", "bin_img");
    pub_debug_info_ = this->declare_parameter("pub_debug_info", true);
    use_uart = this->declare_parameter("use_uart", false);

}

void DetectNode::detect_callback(const sensor_msgs::msg::Image::SharedPtr img_msg, const hnurm_interfaces::msg::VisionRecvData::SharedPtr uart_msg)
{
     static auto last_time = this->now();
    auto        this_time = this->now();
    auto        call_dt   = (this_time - last_time).seconds();
    RCLCPP_INFO(this->get_logger(), "Callback FPS: %f", 1.0 / call_dt);
    last_time = this_time;

    // get image
    static cv_bridge::CvImageConstPtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::BGR8);
    }
    catch(cv_bridge::Exception &e)
    {
        RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
        return;
    }

    // do detect
    rclcpp::Time start      = this->now();

    static long  detect_cnt = 0;

    // detector_->detect(cv_ptr->image, self_color);
    detector_.detect(cv_ptr->image);

    auto dt = (this->now() - start).seconds();

    // pub res
    std_msgs::msg::Float64 dt_;
    dt_.data = dt * 1000.0;
    pub_detect_time_->publish(dt_);
    
    // publish res images
    // if(pub_debug_info_)
    // {
    //     cv::Mat res_img = cv_ptr->image.clone();
    //     cv::Mat bin_img = detector_.bin_img.clone();
    //     detector_.Draw(res_img);

    //     cv::imshow("res", res_img);
    //     // pub_bin_img_.publish(*bin_img_msg);
    //     cv::imshow("bin", bin_img);
    //     cv::waitKey(7);
    // }
    hnurm_interfaces::msg::VisionSendData send;
    send.header.frame_id = "serial_send";
    send.header.stamp = this->now();
    send.control_id = 1.0;  // copy the control idsend_data_pub_
    send.vel_x = 0.0;
    send.vel_y = 0.0;
    send.vel_yaw = 0.0;


    // send offset data
    send.pitch = (float)detector_.is_find;
    send.yaw = detector_.yaw + bias;
    // send.target_state=detector_.is_find;
    pub_send_data_->publish(send);

    

}

void DetectNode::image_callback(const sensor_msgs::msg::Image::SharedPtr img_msg)
{   
    if(!use_uart)
    {
        static auto last_time = this->now();
        auto        this_time = this->now();
        auto        call_dt   = (this_time - last_time).seconds();
        // RCLCPP_INFO(this->get_logger(), "Callback FPS: %f", 1.0 / call_dt);
        last_time = this_time;

        // get image
        static cv_bridge::CvImageConstPtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::BGR8);
        }
        catch(cv_bridge::Exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
            return;
        }
        


        // do detect
        rclcpp::Time start      = this->now();

        static long  detect_cnt = 0;

        // detector_->detect(cv_ptr->image, self_color);
        detector_.detect(cv_ptr->image);

        auto dt = (this->now() - start).seconds();
        
        

        // pub res
        std_msgs::msg::Float64 dt_;
        dt_.data = dt * 1000.0;
        pub_detect_time_->publish(dt_);



        hnurm_interfaces::msg::VisionSendData send;
        send.header.frame_id = "serial_send";
        send.header.stamp = this->now();
        send.control_id = 1.0;  // copy the control idsend_data_pub_
        send.vel_x = 0.0;
        send.vel_y = 0.0;
        send.vel_yaw = 0.0;


        // send offset data
        send.pitch = (float)detector_.is_find;
        send.yaw = detector_.yaw + bias;


        // send.target_state=detector_.is_find;
        pub_send_data_->publish(send);

        cv::Mat res_img = cv_ptr->image.clone();
    //     cv::Mat bin_img = detector_.bin_img.clone();
        detector_.Draw(res_img);
        RCLCPP_INFO(this->get_logger(), "Send Yaw: %f", send.yaw);
        RCLCPP_INFO(this->get_logger(), "Bias Yaw: %f", bias);
        cv::imshow("res", res_img);
    //     // pub_bin_img_.publish(*bin_img_msg);
    //     cv::imshow("bin", bin_img);
        cv::waitKey(7);

    }
     

}


}  // namespace hnurm
